#include "../pid/bsp_pid.h";
#include "../custom/node.h";

int Check_Val(int next) {
	if(next < -99999 || next > 99999) {
		return 0;
	} else {
		return next;
	}
}

int Motor_Calc1(list* ls, int time) {
	

	float val = (float) get_last(ls)->v; 
	int next = (int) speed_pid_realize(&pid_speed, val);
	
	if(time < 15) {
		pid_speed.target_val = 1587;
	} else {
		pid_speed.target_val = 2380;
	}
	return next;
}

int Motor_Calc2(list* ls, int time) {
 
 
	float val = (float) get_last(ls)->v; 
	int next = (int) speed_pid_realize(&pid_speed, val);
	
	if(time > 15) {
		pid_speed.target_val = 1587;
	} else {
		pid_speed.target_val = 2380;
	}
	return next; 
}

// Fibonacci Implement

int Motor_Calc3(list* ls, int time) {
	int* val =  get_last3v(ls);
	
	if(val[0] == 0 && val[1] == 0) {
		return 1;
	} else {
		return val[0]+ val[1];
	}
}

int Motor_Calc4(list* ls, int time) {
	return 0;
}

